package com.bearingnavi.android.util;

import android.graphics.Point;

import com.bearingnavi.android.services.LocationService;
import com.google.android.maps.GeoPoint;

public class Mathematics {

	public static GeoPoint createDestnationPoint(double lat, double lon, double distance, double bearing) {
		
		int R = 6371;
		
		distance = distance/R;  // convert dist to angular distance in radians
		bearing = Math.toRadians(bearing);
		double lat1 = Math.toRadians(lat);
		double lon1 = Math.toRadians(lon);
		
		double lat2 = Math.asin( Math.sin(lat1)*Math.cos(distance) + 
                  Math.cos(lat1)*Math.sin(distance)*Math.cos(bearing) );
		double lon2 = lon1 + Math.atan2(Math.sin(bearing)*Math.sin(distance)*Math.cos(lat1), 
		                         Math.cos(distance)-Math.sin(lat1)*Math.sin(lat2));
		
		lon2 = (lon2+3*Math.PI) % (2*Math.PI) - Math.PI;  // normalise to -180..+180º
		
		lat2 = Math.toDegrees(lat2);
		lon2 = Math.toDegrees(lon2);

		return new GeoPoint(
				(int) (lat2 * 1000000),
				(int) (lon2 * 1000000));
		
	}
	
	public static long calculateDistance(GeoPoint gp1, GeoPoint gp2) {
		
		int R = 6371;
		
		double lat1 = (double) gp1.getLatitudeE6()  / 1000000;
		double lon1 = (double) gp1.getLongitudeE6() / 1000000;
		double lat2 = (double) gp2.getLatitudeE6()  / 1000000;
		double lon2 = (double) gp2.getLongitudeE6() / 1000000;
		
		lat1 = Math.toRadians(lat1);
		lon1 = Math.toRadians(lon1);
		lat2 = Math.toRadians(lat2);
		lon2 = Math.toRadians(lon2);
		
		
		double dLat = lat2 - lat1;
		double dLon = lon2 - lon1;
		
		double a = Math.sin(dLat/2) * Math.sin(dLat/2) +
		          Math.cos(lat1) * Math.cos(lat2) * 
		          Math.sin(dLon/2) * Math.sin(dLon/2);
		double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
		double d = R * c;
		
		return Math.round(d * 1000);
		
	}
	
	// Returns Magnetic north (and not the true north)
	public static long calculateInitialBearing(GeoPoint gp1, GeoPoint gp2) {
		
		double lat1 = (double) gp1.getLatitudeE6()  / 1000000;
		double lon1 = (double) gp1.getLongitudeE6() / 1000000;
		double lat2 = (double) gp2.getLatitudeE6()  / 1000000;
		double lon2 = (double) gp2.getLongitudeE6() / 1000000;

		lat1 = Math.toRadians(lat1);
		lat2 = Math.toRadians(lat2);
		
		double dLon = Math.toRadians(lon2-lon1);

		double y = Math.sin(dLon) * Math.cos(lat2);
		double x = Math.cos(lat1)*Math.sin(lat2) - 
				Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
		double brng = Math.atan2(y, x);
		brng = ((Math.toDegrees(brng) - LocationService.getDeclination(gp1) + 360) % 360);
		
		if (Math.round(brng) == 360){
			brng = 0;
		}
		
		return Math.round(brng);

		
	}
	
	public static float getMidpoint(float a, float b) {
		return (a + b) / 2;
	}


	public static float ClipAngleTo0_360(float angle) {
		if (angle < 0) {
			angle += 360;
		}
		return angle % 360.0f;
	}
	
	
	
	public static int isClosePoints(Point p1, Point p2, int maxDeviation) {
		double radius = Math.sqrt((p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y)); 
		if (radius < maxDeviation) {
			return (int)radius;
		}
		return -1;
	}
	
	public static int max(int a, int b) {
		if (a>b)
			return a;
		return b;
	}
	
	public static int min(int a, int b) {
		if (a<b)
			return a;
		return b;
	}
	
	public static int isPointCloseToLine(Point A, Point B, Point P, int maxDeviation) {
		if (P.x < max(A.x, B.x) && P.x > min(A.x, B.x) &&
				P.y < max(A.y, B.y) && P.y > min(A.y, B.y))
		{
		    double normalLength = Math.sqrt((B.x-A.x)*(B.x-A.x)+(B.y-A.y)*(B.y-A.y));
		    normalLength = Math.abs((P.x-A.x)*(B.y-A.y)-(P.y-A.y)*(B.x-A.x))/normalLength;
		    if (normalLength < maxDeviation)
		    	return (int)normalLength;
		}
		return -1;
	}
}
